#ifndef PLANNERCORE_H
#define PLANNERCORE_H

#include <vector>
#include "mPathNode2d.h"
#include <opencv2/opencv.hpp>
#include <unordered_set>

namespace findPlanner {

struct PointHasher{
  std::size_t operator()(const std::pair<int, int> &point) const{
    std::hash<int> hasher;
    std::size_t seed = 0;
    seed ^= hasher(point.first) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    seed ^= hasher(point.second) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    return seed;
  }
};

struct PointEqual {
  bool operator()(const std::pair<int, int>& p1, const std::pair<int, int>& p2) const {
    return p1.first == p2.first && p1.second == p2.second;
  }
};

typedef std::unordered_set<std::pair<int, int>, PointHasher, PointEqual> mpointData;

class Planner
{
private:
  int width, height;
  cv::Mat mCostMap;
  mpointData voronoiData;
  mNode2d *nodes2D = nullptr;

public:
  Planner() {}
  ~Planner() {}

  void removeMap() { delete[] nodes2D; }
  void initializeLookups(const cv::Mat &map);
  void initializePointData(const mpointData &msg_data) { voronoiData = msg_data; }

  bool astartfindPath(const std::pair<int, int> &start, const std::pair<int, int> &goal, std::vector<std::pair<int, int>> &plan);
  bool findPath(const std::pair<int, int> &start, const std::pair<int, int> &goal, std::vector<std::pair<int, int>> &plan, bool check_cell);
  void smoothPath(std::vector<std::pair<float, float>> *path);

};



} // namespace findPlanner
#endif